import math
import random
from plan_tool.Node import Node
from plan_tool.point import point
from plan_tool.plotmaze import *
import numpy as np
from scipy.spatial import distance


class RRT:  # 定义A*算法中的函数
    def __init__(self, obstacle, startnode, endnode, length, width, step, maxangle, Pg):
        self.leaf = []
        self.path = []

        self.mazeObstacle = obstacle  # 地图信息，记录不可行点
        self.root = startnode
        self.endNode = endnode
        self.length = length
        self.width = width
        self.step = step
        self.maxangle = maxangle
        self.Pg = Pg
        self.randnode = None
        self.currNode = None
        self.tempPG = self.Pg - 3 / 100

    def minDisnode(self):  # 计算leaf节点中的最小距离的节点
        if len(self.leaf) == 0:
            return None
        else:
            minNode = self.leaf[0]
            for tempNode in self.leaf:
                if tempNode.distance < minNode.distance:
                    minNode = tempNode
            return minNode

    def getRandnode(self):
        #print("try")
        percent = random.random()
        if percent >= self.Pg:
            self.randnode = Node(point(random.uniform(0, self.length - 1), random.uniform(0, self.width - 1)))
        else:
            self.randnode = self.endNode

    def getLeafdis(self):
        for tempNode in self.leaf:
            if self.randnode is not None:
                tempNode.getDis(self.randnode)

    def isAccess(self, thisPoint):  # 判断point是否可通行
        if thisPoint.x < 0 or thisPoint.y < 0:  # 低于下界
            return False
        if thisPoint.x >= self.length - 1 or thisPoint.y >= self.width - 1:  # 高于上界
            return False
        for i in self.mazeObstacle:  # 处于障碍物内
            if pow(i[0] - thisPoint.x, 2) + pow(i[1] - thisPoint.y, 2) <= pow(i[2], 2):
                return False
        return True

    def Heron(self, a, b, c):
        p = (a + b + c) / 2
        S = math.sqrt(abs(p * (p - a) * (p - b) * (p - c)))
        h = 2 * S / a
        return h

    def isSafe(self, newPoint, nearNode):
        newNear = self.step
        for i in self.mazeObstacle:
            newDx = newPoint.x - i[0]
            newDy = newPoint.y - i[1]
            nearDx = nearNode.point.x - i[0]
            nearDy = nearNode.point.y - i[1]

            newO = math.sqrt(pow(newDx, 2) + pow(newDy, 2))
            nearO = math.sqrt(pow(nearDx, 2) + pow(nearDy, 2))
            h = self.Heron(newNear, newO, nearO) # 海伦公式
            cosnew = (pow(newNear, 2) + pow(newO, 2) - pow(nearO, 2)) / (2 * newO * newNear)
            cosnear = (pow(newNear, 2) + pow(nearO, 2) - pow(newO, 2)) / (2 * nearO * newNear) # 余弦定理

            if h <= i[2] and cosnew > 0 and cosnear > 0:
                return False

        return True

    # def isSafe(self, newPoint, nearNode):
    #     newNear = self.step
    #     for i in self.mazeObstacle:
    #         # 圆的圆心和半径
    #         a, b, r = i[0], i[1], i[2]
    #         # 新点和旧点的坐标
    #         x1, y1 = newPoint.x, newPoint.y
    #         x2, y2 = nearNode.point.x, nearNode.point.y
    #         # 连线的斜率和截距
    #         k = (y2 - y1) / (x2 - x1)
    #         c = y1 - k * x1
    #         # 联立圆的方程和连线的方程，得到关于x的二次方程
    #         A = 1 + k ** 2
    #         B = 2 * (k * c - k * b - a)
    #         C = a ** 2 + (c - b) ** 2 - r ** 2
    #         # 计算判别式
    #         delta = B ** 2 - 4 * A * C
    #         # 如果判别式小于0，说明无实数根，不相交
    #         if delta < 0:
    #             return True
    #         # 如果判别式大于等于0，说明有实数根，可能相交
    #         else:
    #             # 计算根的值
    #             x3 = (-B + math.sqrt(delta)) / (2 * A)
    #             x4 = (-B - math.sqrt(delta)) / (2 * A)
    #             # 判断根的值是否在连线的范围内
    #             if min(x1, x2) <= x3 <= max(x1, x2) or min(x1, x2) <= x4 <= max(x1, x2):
    #                 return False
    #             # 增加的判断条件：如果两个点和圆心在同一条直线上
    #             elif x1 == x2 == a:
    #                 # 如果两个点的距离小于等于障碍物的直径，说明相交
    #                 if math.hypot(x1 - x2, y1 - y2) <= 2 * r:
    #                     return False
    #             # 增加的判断条件：如果两个点和圆心形成一个钝角
    #             elif (x1 - a) * (x2 - a) + (y1 - b) * (y2 - b) < 0:
    #                 # 如果两个点中离障碍物更近的那个点的距离小于等于障碍物的半径，说明相交
    #                 if min(math.hypot(x1 - a, y1 - b), math.hypot(x2 - a, y2 - b)) <= r:
    #                     return False
    #             # 增加的判断条件：如果两个点和圆心形成一个钝角
    #             elif (x1 - a) * (x2 - a) + (y1 - b) * (y2 - b) < 0:
    #             # 如果两个点中离障碍物更近的那个点的距离小于等于障碍物的半径，说明相交
    #                 if min(math.hypot(x1 - a, y1 - b), math.hypot(x2 - a, y2 - b)) <= r:
    #                     return False
    #             else:
    #                 return True

    def isLast(self, thisNode):
        thisNode.getDis(self.endNode)
        if thisNode.distance <= self.step * 3:
            # print('end')
            return True
        else:
            return False

    def newLeaf(self):
        if len(self.leaf) == 0:
            self.leaf.append(self.root)
            self.currNode = self.root
        else:
            self.getRandnode()  # 获取rand节点
            self.getLeafdis()  # 计算所有叶子节点到rand节点的距离
            nearNode = self.minDisnode()  # 找到距离rand节点距离最近的叶子节点
            randNode = self.randnode  # 将rand节点非self化
            # nearNode.getDis(randNode)  # 再计算一次距离rand节点距离，应该可删

            gapX = randNode.point.x - nearNode.point.x
            gapY = randNode.point.y - nearNode.point.y  # 两点坐标差
            nearNode.getDis(randNode)
            gapD = nearNode.distance  # 两点距离
            sin = gapX / gapD
            cos = gapY / gapD  # 两点角度

            newX = nearNode.point.x + sin * self.step
            newY = nearNode.point.y + cos * self.step
            newPoint = point(newX, newY)  # 按一定步长沿该方向找到新节点

            if self.isAccess(newPoint) is not True:  # 新节点是否可达
                #print("not access")
                if self.Pg >= 0.5:
                    self.Pg = self.Pg - 1 / 100
                return

            if self.isSafe(newPoint, nearNode) is not True:  # 新旧节点之间是否有障碍物
                #print("not safe")
                if self.Pg >= 0.5:
                    self.Pg = self.Pg - 1 / 100
                return

            # if cos <= math.cos(self.maxangle / 180 * math.pi):  # 该路线角度是否过大
            newNode = Node(newPoint)
            newNode.ancientNode(nearNode)
            self.leaf.append(newNode)
            self.currNode = newNode
            if self.Pg < self.tempPG:
                self.Pg = self.Pg + 3 / 100


    def run(self):
        temp = 0
        while True:
            temp = temp + 1
            self.newLeaf()
            if self.isLast(self.currNode) is True:
                # print("last")
                self.endNode.ancient = self.currNode
                self.endNode.point.x = self.endNode.point.x - 1
                self.endNode.point.y = self.endNode.point.y - 1
                tempNode = self.endNode
                while True:
                    self.path.append(tempNode)
                    if tempNode.ancient != None:
                        tempNode = tempNode.ancient
                    else:
                        return self.path
            elif len(self.leaf) >= self.length * self.width * 0.1:
                return False
